/*
 * Copyright (c) 2012-2012, L.T. Jiang
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the University of Freiburg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (IN1CLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <tabletop_octomap/TabletopOctomapServer.h>

using namespace octomap;

namespace octomap_server{



/*////////////////////////////////////////////////////////////////////////////////////////
* Constructor
*/////////////////////////////////////////////////////////////////////////////////////////
TabletopOctomapServer::TabletopOctomapServer()
: OctomapServer(),
priv_nh_("~"),
m_tabletopProcessingFrame("camera_rgb_optical_frame")
{
	//tabletop octomap processing service
  m_tabletopProcessingService = m_nh.advertiseService("tabletop_octomap", 
												&TabletopOctomapServer::tabletopProcessingSrv, this);

  //initialize parameters
  priv_nh_.param<int>("inlier_threshold", inlier_threshold_, 300);
  priv_nh_.param<double>("plane_detection_voxel_size", plane_detection_voxel_size_, 0.01);
  priv_nh_.param<double>("clustering_voxel_size", clustering_voxel_size_, 0.003);
  priv_nh_.param<double>("z_filter_min", z_filter_min_, 0.4);
  priv_nh_.param<double>("z_filter_max", z_filter_max_, 1.25);
  priv_nh_.param<double>("y_filter_min", y_filter_min_, -1.0);
  priv_nh_.param<double>("y_filter_max", y_filter_max_, 1.0);
  priv_nh_.param<double>("x_filter_min", x_filter_min_, -1.0);
  priv_nh_.param<double>("x_filter_max", x_filter_max_, 1.0);
  priv_nh_.param<double>("table_z_filter_min", table_z_filter_min_, 0.01);
  priv_nh_.param<double>("table_z_filter_max", table_z_filter_max_, 0.50);
  priv_nh_.param<double>("cluster_distance", cluster_distance_, 0.03);
  priv_nh_.param<int>("min_cluster_size", min_cluster_size_, 300);
  priv_nh_.param<std::string>("processing_frame", processing_frame_, "");
  priv_nh_.param<double>("up_direction", up_direction_, -1.0);
  priv_nh_.param<bool>("flatten_table", flatten_table_, false);
  priv_nh_.param<double>("table_padding", table_padding_, 0.0);
  if(flatten_table_) ROS_DEBUG("flatten_table is true");
  else ROS_DEBUG("flatten_table is false");

}



/*////////////////////////////////////////////////////////////////////////////////////////
* Destructor
*/////////////////////////////////////////////////////////////////////////////////////////
TabletopOctomapServer::~TabletopOctomapServer() {}


/*////////////////////////////////////////////////////////////////////////////////////////
* Service callback for the tabletop_octomap service 
*/////////////////////////////////////////////////////////////////////////////////////////
bool TabletopOctomapServer::tabletopProcessingSrv(std_srvs::Empty::Request& req,
                                          std_srvs::Empty::Response& resp) {

	//take a snapshot of the PointCloud2
	sensor_msgs::PointCloud2 snapshot_pc2 = getPointCloud2(); 

	//get PCL PointCloud of the table and objects from the whole PointCloud2
	PCLPointCloud table_pc;
	PCLPointCloud object_pc;
	objectDetection(snapshot_pc2, object_pc, table_pc);
	
	//verify the pointcloud
	std::cout << "snapshot pointcloud size= " << snapshot_pc2.width << std::endl;
	std::cout << "table pointcloud size= " << table_pc.size() << std::endl;
	std::cout << "object pointcloud size= " << object_pc.size() << std::endl;



	return true; 
}


/*////////////////////////////////////////////////////////////////////////////////////////
* 
*
*
*
*/////////////////////////////////////////////////////////////////////////////////////////
sensor_msgs::PointCloud2 TabletopOctomapServer::getPointCloud2() {

// -------------------- Step 1: Get a PointCloud2 from "cloud_in" ------------------ //
	ros::Time start_time = ros::Time::now();
	std::string topic = "cloud_in";
  ROS_INFO("objectDetection called; waiting for a point_cloud2 on topic %s", topic.c_str());
  sensor_msgs::PointCloud2::ConstPtr recent_cloud =
    ros::topic::waitForMessage<sensor_msgs::PointCloud2>(topic, m_nh, ros::Duration(3.0));

	if (!recent_cloud)
  {
    ROS_ERROR("objectDetection:: no PointCloud2 has been received");
    //return true;
  }

  ROS_INFO_STREAM("Point cloud received after " << ros::Time::now() - start_time << " seconds; processing");


// ------------- step 2: transform pointcloud2 to m_tabletopprocessingframe ---------//
  sensor_msgs::PointCloud old_cloud;
  sensor_msgs::convertPointCloud2ToPointCloud (*recent_cloud, old_cloud);
  int current_try=0, max_tries = 3;
  while (1)
  {
    bool transform_success = true;
    try
    {
      m_tfListener.transformPointCloud(m_tabletopProcessingFrame, old_cloud, old_cloud);
    }
    catch (tf::TransformException ex)
    {
      transform_success = false;
      if (++current_try >= max_tries)
      {
        ROS_ERROR("Failed to transform cloud from frame %s into frame %s in %d attempt(s)", 
									 old_cloud.header.frame_id.c_str(), m_tabletopProcessingFrame.c_str(), 
																																							current_try);
        //return true;
      }
      ROS_DEBUG("Failed to transform point cloud, attempt %d out of %d, exception: %s", current_try, max_tries, ex.what());
      //sleep a bit to give the listener a chance to get a new transform
      ros::Duration(0.1).sleep();
    }
    if (transform_success) break;
  }
  sensor_msgs::PointCloud2 converted_cloud;
  sensor_msgs::convertPointCloudToPointCloud2 (old_cloud, converted_cloud);
  ROS_INFO_STREAM("Input cloud converted to " << m_tabletopProcessingFrame << " frame after " <<
                  ros::Time::now() - start_time << " seconds");


// ------------------------ step 3: return the PointCloud2 ------------------------//
	return converted_cloud;
}



/*////////////////////////////////////////////////////////////////////////////////////////
* 
*
*
*
*/////////////////////////////////////////////////////////////////////////////////////////

void TabletopOctomapServer::objectDetection(const sensor_msgs::PointCloud2 &cloud,
																													PCLPointCloud object_pc,
																														PCLPointCloud table_pc)
{
  ROS_INFO("Starting process on new cloud in frame %s", cloud.header.frame_id.c_str());

  // PCL objects
  KdTreePtr normals_tree_, clusters_tree_;
  pcl::VoxelGrid<Point> grid_, grid_objects_;
  pcl::PassThrough<Point> pass_;
  pcl::NormalEstimation<Point, pcl::Normal> n3d_;
  pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
  pcl::ProjectInliers<Point> proj_;
  pcl::ConvexHull<Point> hull_;
  pcl::ExtractPolygonalPrismData<Point> prism_;
  pcl::EuclideanClusterExtraction<Point> pcl_cluster_;
  pcl::PointCloud<Point>::Ptr table_hull_ptr (new pcl::PointCloud<Point>);

  // Filtering parameters
  grid_.setLeafSize (plane_detection_voxel_size_, plane_detection_voxel_size_, plane_detection_voxel_size_);
  grid_objects_.setLeafSize (clustering_voxel_size_, clustering_voxel_size_, clustering_voxel_size_);
  grid_.setFilterFieldName ("z");
  grid_.setFilterLimits (z_filter_min_, z_filter_max_);
  grid_.setDownsampleAllData (false);
  grid_objects_.setDownsampleAllData (true);

  normals_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();
  clusters_tree_ = boost::make_shared<pcl::search::KdTree<Point> > ();

  // Normal estimation parameters
  n3d_.setKSearch (10);
  n3d_.setSearchMethod (normals_tree_);
  // Table model fitting parameters
  seg_.setDistanceThreshold (0.05);
  seg_.setMaxIterations (10000);
  seg_.setNormalDistanceWeight (0.1);
  seg_.setOptimizeCoefficients (true);
  seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
  seg_.setMethodType (pcl::SAC_RANSAC);
  seg_.setProbability (0.99);

  proj_.setModelType (pcl::SACMODEL_PLANE);

  // Clustering parameters
  pcl_cluster_.setClusterTolerance (cluster_distance_);
  pcl_cluster_.setMinClusterSize (min_cluster_size_);
  pcl_cluster_.setSearchMethod (clusters_tree_);

  // Step 1 : Filter, remove NaNs and downsample
  pcl::PointCloud<Point>::Ptr cloud_ptr (new pcl::PointCloud<Point>);
  pcl::fromROSMsg (cloud, *cloud_ptr);
  pass_.setInputCloud (cloud_ptr);
  pass_.setFilterFieldName ("z");
  pass_.setFilterLimits (z_filter_min_, z_filter_max_);
  pcl::PointCloud<Point>::Ptr z_cloud_filtered_ptr (new pcl::PointCloud<Point>);
  pass_.filter (*z_cloud_filtered_ptr);

  pass_.setInputCloud (z_cloud_filtered_ptr);
  pass_.setFilterFieldName ("y");
  pass_.setFilterLimits (y_filter_min_, y_filter_max_);
  pcl::PointCloud<Point>::Ptr y_cloud_filtered_ptr (new pcl::PointCloud<Point>);
  pass_.filter (*y_cloud_filtered_ptr);

  pass_.setInputCloud (y_cloud_filtered_ptr);
  pass_.setFilterFieldName ("x");
  pass_.setFilterLimits (x_filter_min_, x_filter_max_);
  pcl::PointCloud<Point>::Ptr cloud_filtered_ptr (new pcl::PointCloud<Point>);
  pass_.filter (*cloud_filtered_ptr);

  ROS_INFO("Step 1 done");
  if (cloud_filtered_ptr->points.size() < (unsigned int)min_cluster_size_)
  {
    ROS_INFO("Filtered cloud only has %d points", (int)cloud_filtered_ptr->points.size());
    return;
  }

  pcl::PointCloud<Point>::Ptr cloud_downsampled_ptr (new pcl::PointCloud<Point>);
  grid_.setInputCloud (cloud_filtered_ptr);
  grid_.filter (*cloud_downsampled_ptr);
  if (cloud_downsampled_ptr->points.size() < (unsigned int)min_cluster_size_)
  {
    ROS_INFO("Downsampled cloud only has %d points", (int)cloud_downsampled_ptr->points.size());
    return;
  }

  // Step 2 : Estimate normals
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals_ptr (new pcl::PointCloud<pcl::Normal>);
  n3d_.setInputCloud (cloud_downsampled_ptr);
  n3d_.compute (*cloud_normals_ptr);
  ROS_INFO("Step 2 done");

  // Step 3 : Perform planar segmentation
  tf::Transform table_plane_trans;
  tf::Transform table_plane_trans_flat;

    pcl::PointIndices::Ptr table_inliers_ptr (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr table_coefficients_ptr (new pcl::ModelCoefficients);
    seg_.setInputCloud (cloud_downsampled_ptr);
    seg_.setInputNormals (cloud_normals_ptr);
    seg_.segment (*table_inliers_ptr, *table_coefficients_ptr);

    if (table_coefficients_ptr->values.size () <=3)
    {
      ROS_INFO("Failed to detect table in scan");
      return;
    }

    if ( table_inliers_ptr->indices.size() < (unsigned int)inlier_threshold_)
    {
      ROS_INFO("Plane detection has %d inliers, below min threshold of %d", 
								(int)table_inliers_ptr->indices.size(), inlier_threshold_);
      return;
    }

    ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].",
              (int)table_inliers_ptr->indices.size (),
              table_coefficients_ptr->values[0], table_coefficients_ptr->values[1],
              table_coefficients_ptr->values[2], table_coefficients_ptr->values[3]);
    ROS_INFO("Step 3 done");

    // Step 4 : Project the table inliers on the table
    pcl::PointCloud<Point>::Ptr table_projected_ptr (new pcl::PointCloud<Point>);
    proj_.setInputCloud (cloud_downsampled_ptr);
    proj_.setIndices (table_inliers_ptr);
    proj_.setModelCoefficients (table_coefficients_ptr);
    proj_.filter (*table_projected_ptr); //LT: return table_projected_ptr for table?
		table_pc.resize(table_projected_ptr->size());
    for (size_t i = 0; i < table_projected_ptr->points.size(); i++) {
    	table_pc.points[i].x = table_projected_ptr->points[i].x;
      table_pc.points[i].y = table_projected_ptr->points[i].y;
      table_pc.points[i].z = table_projected_ptr->points[i].z;
    }


    ROS_INFO("Step 4 done");

    sensor_msgs::PointCloud table_points;
    sensor_msgs::PointCloud table_hull_points;
    table_plane_trans = getPlaneTransform (*table_coefficients_ptr, up_direction_, false);

    // ---[ Estimate the convex hull (not in table frame)
    hull_.setInputCloud (table_projected_ptr);
    hull_.reconstruct (*table_hull_ptr);

    if(!flatten_table_)
    {
      // --- [ Take the points projected on the table and transform them into the PointCloud message
      //  while also transforming them into the table's coordinate system
      if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans, table_points))
      {
        return;
      }

      // ---[ Convert the convex hull points to table frame
      if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans, table_hull_points))
      {
        return;
      }
    }
    if(flatten_table_)
    {
      // if flattening the table, find the center of the convex hull and move the table frame there
      table_plane_trans_flat = getPlaneTransform (*table_coefficients_ptr, up_direction_, flatten_table_);
      tf::Vector3 flat_table_pos;
      double avg_x, avg_y, avg_z;
      avg_x = avg_y = avg_z = 0;
      for (size_t i=0; i<table_projected_ptr->points.size(); i++)
      {
        avg_x += table_projected_ptr->points[i].x;
        avg_y += table_projected_ptr->points[i].y;
        avg_z += table_projected_ptr->points[i].z;
      }
      avg_x /= table_projected_ptr->points.size();
      avg_y /= table_projected_ptr->points.size();
      avg_z /= table_projected_ptr->points.size();
      ROS_INFO("average x,y,z = (%.5f, %.5f, %.5f)", avg_x, avg_y, avg_z);

      // place the new table frame in the center of the convex hull
      flat_table_pos[0] = avg_x;
      flat_table_pos[1] = avg_y;
      flat_table_pos[2] = avg_z;
      table_plane_trans_flat.setOrigin(flat_table_pos);

      // shift the non-flat table frame to the center of the convex hull as well
      table_plane_trans.setOrigin(flat_table_pos);

      // --- [ Take the points projected on the table and transform them into the PointCloud message
      //  while also transforming them into the flat table's coordinate system
      sensor_msgs::PointCloud flat_table_points;
      if (!getPlanePoints<Point> (*table_projected_ptr, table_plane_trans_flat, flat_table_points))
      {
        return;
      }

      // ---[ Convert the convex hull points to flat table frame
      if (!getPlanePoints<Point> (*table_hull_ptr, table_plane_trans_flat, table_hull_points))
      {
        return;
      }
    }
    ROS_INFO("Table computed");
  
  // Step 5: Get the objects on top of the (non-flat) table
  pcl::PointIndices cloud_object_indices;
  //prism_.setInputCloud (cloud_all_minus_table_ptr);
  prism_.setInputCloud (cloud_filtered_ptr);
  prism_.setInputPlanarHull (table_hull_ptr);
  ROS_INFO("Using table prism: %f to %f", table_z_filter_min_, table_z_filter_max_);
  prism_.setHeightLimits (table_z_filter_min_, table_z_filter_max_);
  prism_.segment (cloud_object_indices);

  pcl::PointCloud<Point>::Ptr cloud_objects_ptr (new pcl::PointCloud<Point>);
  pcl::ExtractIndices<Point> extract_object_indices;
  extract_object_indices.setInputCloud (cloud_filtered_ptr);
  extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
  extract_object_indices.filter (*cloud_objects_ptr);

  ROS_INFO (" Number of object point candidates: %d.", (int)cloud_objects_ptr->points.size ());

  if (cloud_objects_ptr->points.empty ())
  {
    ROS_INFO("No objects on table");
    return;
  }

  // ---[ Downsample the points
  pcl::PointCloud<Point>::Ptr cloud_objects_downsampled_ptr (new pcl::PointCloud<Point>);
  grid_objects_.setInputCloud (cloud_objects_ptr);
  grid_objects_.filter (*cloud_objects_downsampled_ptr);

	/*
  // ---[ If flattening the table, adjust the points on the table to be straight also
  if(flatten_table_) straightenPoints<pcl::PointCloud<Point> >(*cloud_objects_downsampled_ptr,
                       table_plane_trans, table_plane_trans_flat);
	*/
  // Step 6: Split the objects into Euclidean clusters
  std::vector<pcl::PointIndices> clusters2;
  //pcl_cluster_.setInputCloud (cloud_objects_ptr);
  pcl_cluster_.setInputCloud (cloud_objects_downsampled_ptr);
  pcl_cluster_.extract (clusters2);
  ROS_INFO ("Number of clusters found matching the given constraints: %d.", (int)clusters2.size ());

  // ---[ Convert clusters into the PointCloud message
  std::vector<sensor_msgs::PointCloud> clusters;
	clusters.resize(clusters2.size());
	for (size_t i = 0; i < clusters2.size (); ++i)
  {	

		ROS_INFO("The %d cluster", i);
    pcl::PointCloud<Point> cloud_cluster;
		ROS_INFO("1");
    pcl::copyPointCloud(*cloud_objects_downsampled_ptr, clusters2[i], cloud_cluster);
		ROS_INFO("2");
    sensor_msgs::PointCloud2 pc2;
		ROS_INFO("3");
    pcl::toROSMsg( cloud_cluster, pc2 );
		ROS_INFO("4");
    sensor_msgs::convertPointCloud2ToPointCloud (pc2, clusters[i]); //crash here
		ROS_INFO("5");

		
		//only return the 1st object cloud for now
		if (i == 0) {
			object_pc.resize(cloud_cluster.size());
			for (size_t i = 0; i < cloud_cluster.points.size(); i++) {
    		object_pc.points[i].x = cloud_cluster.points[i].x;
    		object_pc.points[i].y = cloud_cluster.points[i].y;
    		object_pc.points[i].z = cloud_cluster.points[i].z;
			}			
			//pcl::copyPointCloud(cloud_cluster, object_pc);
		}
		
  }
  ROS_INFO("Clusters converted");

	//markers for rviz
  //publishClusterMarkers(clusters, cloud.header);

}


/*! Assumes plane coefficients are of the form ax+by+cz+d=0, normalized */
tf::Transform TabletopOctomapServer::getPlaneTransform (pcl::ModelCoefficients coeffs, double up_direction, bool flatten_plane)
{
  ROS_ASSERT(coeffs.values.size() > 3);
  double a = coeffs.values[0], b = coeffs.values[1], c = coeffs.values[2], d = coeffs.values[3];
  //asume plane coefficients are normalized
  tf::Vector3 position(-a*d, -b*d, -c*d);
  tf::Vector3 z(a, b, c);

  //if we are flattening the plane, make z just be (0,0,up_direction)
  if(flatten_plane)
  {
    ROS_INFO("flattening plane");
    z[0] = z[1] = 0;
    z[2] = up_direction;
  }
  else
  {
    //make sure z points "up"
    ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
    if ( z.dot( tf::Vector3(0, 0, up_direction) ) < 0)
    {
      z = -1.0 * z;
      ROS_INFO("flipped z");
    }
  }

  //try to align the x axis with the x axis of the original frame
  //or the y axis if z and x are too close too each other
  tf::Vector3 x(1, 0, 0);
  if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = tf::Vector3(0, 1, 0);
  tf::Vector3 y = z.cross(x).normalized();
  x = y.cross(z).normalized();

  tf::Matrix3x3 rotation;
  rotation[0] = x;  // x
  rotation[1] = y;  // y
  rotation[2] = z;  // z
  rotation = rotation.transpose();
  tf::Quaternion orientation;
  rotation.getRotation(orientation);
  ROS_DEBUG("in getPlaneTransform, x: %0.3f, %0.3f, %0.3f", x[0], x[1], x[2]);
  ROS_DEBUG("in getPlaneTransform, y: %0.3f, %0.3f, %0.3f", y[0], y[1], y[2]);
  ROS_DEBUG("in getPlaneTransform, z: %0.3f, %0.3f, %0.3f", z[0], z[1], z[2]);
  return tf::Transform(orientation, position);
}


template <typename PointT>
bool TabletopOctomapServer::getPlanePoints (const pcl::PointCloud<PointT> &table,
         const tf::Transform& table_plane_trans,
         sensor_msgs::PointCloud &table_points)
{
  // Prepare the output
  table_points.header = table.header;
  table_points.points.resize(table.points.size ());
  for (size_t i = 0; i < table.points.size (); ++i)
  {
    table_points.points[i].x = table.points[i].x;
    table_points.points[i].y = table.points[i].y;
    table_points.points[i].z = table.points[i].z;
  }

  // Transform the data
  tf::TransformListener listener;
  tf::StampedTransform table_pose_frame(table_plane_trans, table.header.stamp,
                                        table.header.frame_id, "table_frame");
  listener.setTransform(table_pose_frame);
  std::string error_msg;
  if (!listener.canTransform("table_frame", table_points.header.frame_id, table_points.header.stamp, &error_msg))
  {
    ROS_ERROR("Can not transform point cloud from frame %s to table frame; error %s",
        table_points.header.frame_id.c_str(), error_msg.c_str());
    return false;
  }
  int current_try=0, max_tries = 3;
  while (1)
  {
    bool transform_success = true;
    try
    {
      listener.transformPointCloud("table_frame", table_points, table_points);
    }
    catch (tf::TransformException ex)
    {
      transform_success = false;
      if ( ++current_try >= max_tries )
      {
        ROS_ERROR("Failed to transform point cloud from frame %s into table_frame; error %s",
                  table_points.header.frame_id.c_str(), ex.what());
        return false;
      }
      //sleep a bit to give the listener a chance to get a new transform
      ros::Duration(0.1).sleep();
    }
    if (transform_success) break;
  }
  table_points.header.stamp = table.header.stamp;
  table_points.header.frame_id = "table_frame";
  return true;
}


template <class PointCloudType>
void TabletopOctomapServer::publishClusterMarkers(const std::vector<PointCloudType> &clusters, std_msgs::Header cloud_header)
{
  for (size_t i=0; i<clusters.size(); i++)
  {
    visualization_msgs::Marker cloud_marker = tabletop_object_detector::MarkerGenerator::getCloudMarker(clusters[i]);
    cloud_marker.header = cloud_header;
    cloud_marker.pose.orientation.w = 1;
    cloud_marker.ns = "tabletop_node";
    cloud_marker.id = current_marker_id_++;
    m_markerPub.publish(cloud_marker);
  }
}



void TabletopOctomapServer::insertCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud) {
	//ROS_INFO("Do Nothing");
}
}



